package com.jmex.physics.impl.jbullet;

import com.bulletphysics.linearmath.MotionState;
import com.bulletphysics.linearmath.Transform;
import com.jmex.physics.impl.jbullet.util.VecmathConverter;

public class NodeLockedMotionState implements MotionState {

	private JBulletPhysicsNode node;
	private final Transform graphicsWorldTrans = new Transform();
	public final Transform centerOfMassOffset = new Transform();
	public final Transform startWorldTrans = new Transform();
	
	private NodeLockedMotionState(){}
	
	public NodeLockedMotionState(JBulletPhysicsNode node)
	{
		this(node, null, null);
	}
	
	public NodeLockedMotionState(JBulletPhysicsNode node, Transform startWorldTrans, Transform centerOfMassOffset)
	{
		this.node = node;
		graphicsWorldTrans.setIdentity();
		if(centerOfMassOffset==null) this.centerOfMassOffset.setIdentity();
		if(startWorldTrans==null) this.startWorldTrans.setIdentity();
	}
	
	public void getWorldTransform(Transform centerOfMassWorldTrans) {
		graphicsWorldTrans.setIdentity();
		if(node.getParent()!=null)
		{
			graphicsWorldTrans.origin.set(VecmathConverter.convert(node.getParent().getWorldTranslation().add(node.getWorldTranslation())));
			graphicsWorldTrans.setRotation(VecmathConverter.convert(node.getWorldRotation().mult(node.getParent().getWorldRotation())));
		} else {
			graphicsWorldTrans.origin.set(VecmathConverter.convert(node.getWorldTranslation()));
			graphicsWorldTrans.setRotation(VecmathConverter.convert(node.getWorldRotation()));
		}
		centerOfMassWorldTrans.inverse(centerOfMassOffset);
		centerOfMassWorldTrans.mul(graphicsWorldTrans);
	}
	
	public void setWorldTransform(Transform centerOfMassWorldTrans) {
		graphicsWorldTrans.set(centerOfMassWorldTrans);
		graphicsWorldTrans.mul(centerOfMassOffset);
		if(node.getParent()!=null)
		{
			node.setLocalTranslation(VecmathConverter.convert(graphicsWorldTrans.origin).subtract(node.getParent().getWorldTranslation()),true);
			node.setLocalRotation(VecmathConverter.convert(graphicsWorldTrans.getRotation()).mult(node.getParent().getWorldRotation().inverse()),true);
		} else {
			node.setLocalTranslation(VecmathConverter.convert(graphicsWorldTrans.origin),true);
			node.setLocalRotation(VecmathConverter.convert(graphicsWorldTrans.getRotation()),true);
		}
	}
}
